package frc.circuitrunners.RobotSubsystems.Shooter;

/** Imports */
import edu.wpi.first.wpilibj.Victor;
import frc.circuitrunners.IO.IO;
import frc.circuitrunners.R;
import frc.circuitrunners.RobotSubsystems.RobotSubsystem;
import frc.circuitrunners.Scheduler;

/**
 * @author CircuitRunners
 * This package and class contain all implementation of the Shooter code
 */

public class Shooter extends RobotSubsystem{
    
    private Victor rotationMotor, shootingMotor;
    private int ballCount=0;
    
    public void feed(){
        ballCount++;
    }
    
    public void runAutonomous() {
        if(!flag){
            switch(R.STAGE){
                case R.stage.PREPARATION:
                    flag = true;
                    Scheduler.yield();
                case R.stage.SHOOT:
                    flag = true;
                    Scheduler.yield();
                case R.stage.BACKUP:
                    flag = true;
                    Scheduler.yield();
                case R.stage.BALL_SMASH:
                    flag = true;
                    Scheduler.yield();
            }
        }
    }
    
    /**
     * Initializes the Shooter
     */
    public void init() {
        rotationMotor = new Victor(2,3); //Initialize rotation motor
        shootingMotor = new Victor(2,4); //Initialize shooting motor
        //setFlag(IO.Shooter == 1.0 ? true : false;
    }

    /**
     * The looped Run method for Shooter
     */
    public void runTeleop() {
        Scheduler.feed();
        if(IO.getInput(IO.JOY_RIGHT | IO.BUTTON | 1) == 1.0){ //checks the right trigger for pressed or not
            shootingMotor.set(1.0); //if pressed, run it forward
        }
    }
    
    /**
     * Disables the shooter 
     */
    public void disable(){
        Scheduler.feed();
        rotationMotor.set(0.0); //Disable rotation motor
        shootingMotor.set(0.0); //Disable shooting motor
    }

    /**
     * Calibrates the shooter for automatically during AutoAim
     */
    public void autoAim() {
        double distanceToGoal = IO.getEstimatedDistance(); //Gets the estimated distance
        rotationMotor.set(IO.getPIDoutput()); //Moves rotation motor based on how camera input
        if(IO.getInput(IO.JOY_RIGHT | IO.BUTTON | 1) == 1.0){ // if right trigger is pressed
            shootingMotor.set(distanceToGoal / 10); //turn at speed for pin point shooting
        }
    }
}
